#include <math.h>

#include "memory.h"
#include "quaternion.h"

OPERATOR_NEW_OVERLOAD(Quaternion)

void Quaternion::print_ht(ostream& os, char *title)
{
	os << "<table><tbody><tr><th>" << title << "</th><td>" 
		<< coord[VX] << "</td><td>"
		<< coord[VY] << "</td><td>"
		<< coord[VZ] << "</td><td>"
		<< coord[S] << "</td></tr></tbody></table>";
}

void Quaternion::Multiply(Quaternion& a, Quaternion& b)
{
	coord[VX] = a[0]*b[0] - (a[1]*b[1] + a[2]*b[2] + a[3]*b[3]);
	coord[VY] = a[0]*b[1] + b[0]*a[1] + a[2]*b[3] - a[3]*b[2];
	coord[VZ] = a[0]*b[2] + b[0]*a[2] + a[3]*b[1] - a[1]*b[3];
	coord[S] = a[0]*b[3] + b[0]*a[3] + a[1]*b[2] - a[2]*b[1];
}

Quaternion operator*(Triple& omega, Quaternion& q)
{
	Quaternion result;
#ifdef SSSE3__
	__asm
	{
		;// load
		mov eax,omega
			movaps xmm0,[eax]
		mov eax,q
			movaps xmm1,[eax]

		;// broadcast 'omega'
		pshufd xmm4,xmm0,0x00      ;// [0]
		pshufd xmm5,xmm0,0x55      ;// [1]
		pshufd xmm6,xmm0,0xAA      ;// [2]

		;// 12 multiplications
		mulps xmm4,xmm1
			mulps xmm5,xmm1
			mulps xmm6,xmm1

			;// shuffle
		shufps xmm4,xmm4,0xB1  ;// 0b10110001
		shufps xmm5,xmm5,0x4E  ;// 0b01001110
		shufps xmm6,xmm6,0x1B  ;// 0b00011011

		;// prepare for changing the sign -- load -0.0f
		mov ecx,0x80000000
			movd xmm3,ecx

			;// prepare individual masks
		pshufd xmm0,xmm3,0xCC  ;// 0x11001100
		pshufd xmm1,xmm3,0x3C  ;// 0x00111100
		pshufd xmm2,xmm3,0xF0  ;// 0x11110000

		;// change sign
		xorps xmm4,xmm0
			xorps xmm5,xmm1
			xorps xmm6,xmm2

			;// add
		addps xmm4,xmm5
			addps xmm4,xmm6

			;// /2
		mov ecx,0x3F000000     ;// 0.5f
		movd xmm5,ecx
			shufps xmm5,xmm5,0x00
			mulps xmm4,xmm5

			;// store
		mov eax,dq
			movaps [eax],xmm4
	}
#else
	result[0] = -(omega[0]*q[1] + omega[1]*q[2] + omega[2]*q[3]);
	result[1] = q[0]*omega[0] + omega[1]*q[3] - omega[2]*q[2];
	result[2] = q[0]*omega[1] + omega[2]*q[1] - omega[0]*q[3];
	result[3] = q[0]*omega[2] + omega[0]*q[2] - omega[1]*q[1];

	result *= 0.5f;

	return result;
#endif
}

void Quaternion::operator*=(const float s)
{
#ifdef SSSE3__
	const void *dest = this->coord;
	__asm
	{
		movss xmm1,s                            ;// xmm1 <= s
		shufps xmm1,xmm1,0x00

		mov eax,dest
			movaps xmm0,xmmword ptr [eax]
		mulps xmm0,xmm1
			movaps xmmword ptr [eax],xmm0
	}
#else
	for (int i = 0; i < 4; i++)
	{
	    coord[i] = s * coord[i];
	}
#endif
}

void Quaternion::operator=(Matrix& m)
{
	float tr, s;

	tr = m[0][0] + m[1][1] + m[2][2]; // dot product

	if(tr >= 0)
	{
		s = sqrt(tr + 1.0f);
		coord[0] = 0.5f * s;
		s = 0.5f / s;
		coord[1] = (m[2][1] - m[1][2]) * s;
		coord[2] = (m[0][2] - m[2][0]) * s;
		coord[3] = (m[1][0] - m[0][1]) * s;
	}
	else
	{
		int i = 0;
		if(m[1][1] > m[0][0])
			i = 1;
		if(m[2][2] > m[i][i])
			i = 2;
		switch (i)
		{
		case 0:
			s = sqrt((m[0][0] - (m[1][1] + m[2][2])) + 1.0f);
			coord[1] = 0.5f * s;
			s = 0.5f / s;
			coord[2] = (m[0][1] + m[1][0]) * s;
			coord[3] = (m[2][0] + m[0][2]) * s;
			coord[0] = (m[2][1] - m[1][2]) * s;
			break;
		case 1:
			s = sqrt((m[1][1] - (m[2][2] + m[0][0])) + 1.0f);
			coord[2] = 0.5f * s;
			s = 0.5f / s;
			coord[3] = (m[1][2] + m[2][1]) * s;
			coord[1] = (m[0][1] + m[1][0]) * s;
			coord[0] = (m[0][2] - m[2][0]) * s;
			break;
		case 2:
			s = sqrt((m[2][2] - (m[0][0] + m[1][1])) + 1.0f);
			coord[3] = 0.5f * s;
			s = 0.5f / s;
			coord[1] = (m[2][0] + m[0][2]) * s;
			coord[2] = (m[1][2] + m[2][1]) * s;
			coord[0] = (m[1][0] - m[0][1]) * s;
		}
	}
}

// best time = 284
// SSSE3 best time = 175
// SSSE3 approx. best time = 110
void Quaternion::Normalize()
{
#ifdef SSSE3
	const void *dest = this->coord;
	__asm
	{
		mov eax,dest
			movaps xmm0,[eax]
		movaps xmm1,xmm0          ; // copy quaternion in xmm1

		; // dot product
		mulps xmm0,xmm1
			haddps xmm0,xmm0
			haddps xmm0,xmm0

#ifndef APPROX
			sqrtss xmm2,xmm0
			shufps xmm2,xmm2,0x00
			divps xmm1,xmm2
#else
			rsqrtss xmm2,xmm0
			shufps xmm2,xmm2,0x00
			mulps xmm1,xmm2
#endif
			movaps [eax],xmm1
	}
#else
	float u = 0.0;
	for (int i = 0; i < 4; i++)
	{
		u += coord[i]*coord[i];
	}
	u = sqrt(u);
	for (int i = 0; i < 4; i++)
	{
		coord[i] /= u;
	}
#endif
}

#ifdef SSSE3
// helper function
inline void transpose_xmm0123()
{
	//__asm // [SSE4.1 required; true transposition; firmware]
	//{
	//	;// lines 0-1
	//	shufps xmm1,xmm1,0xB1 ;// rotate 2 and 2
	//	movaps xmm4,xmm0 ;// save (temp is xmm4)
	//	blendps xmm0,xmm1,0x05
	//	blendps xmm1,xmm4,0x05
	//	shufps xmm1,xmm1,0xB1 ;// rotate back 2 and 2

	//	;// lines 2-3
	//	shufps xmm3,xmm3,0xB1 ;// rotate
	//	movaps xmm5,xmm2 ;// save (temp is xmm4)
	//	blendps xmm2,xmm3,0x05
	//	blendps xmm3,xmm5,0x05
	//	shufps xmm3,xmm3,0xB1

	//	;// upper-right and bottom-left corners
	//	movaps xmm4,xmm2 ;// save
	//	movhlps xmm0,xmm4
	//	movlhps xmm3,xmm0

	//	movaps xmm5,xmm3 ;// save
	//	movhlps xmm1,xmm5
	//	movlhps xmm3,xmm1
	//}
	__asm // [fast; false transposition; blurry (difficult to understand); alters xmm4, xmm6]
	{
		movaps xmm4,xmm0       ;// save
		movaps xmm6,xmm2       ;// save

		unpckhps xmm0,xmm1
			unpckhps xmm2,xmm3

			unpcklps xmm1,xmm4
			unpcklps xmm3,xmm6

			movaps xmm4,xmm0       ;// save
		shufps xmm0,xmm2,0xEE  ;// z[] in xmm0 now (w[])
		shufps xmm4,xmm2,0x44  ;// y[] in xmm4 now (z[])

		movaps xmm6,xmm1       ;// save
		shufps xmm1,xmm3,0xBB  ;// x[] in xmm1 now (y[])
		shufps xmm6,xmm3,0x11  ;// s[] in xmm6 now (x[])

		movaps xmm3,xmm0
			movaps xmm0,xmm6
			movaps xmm2,xmm4

			//// old variant
			//movaps xmm4,xmm0       ; save
			//movaps xmm5,xmm2       ; save

			//unpckhps xmm0,xmm1
			//unpckhps xmm2,xmm3
			//movaps xmm6,xmm0       ; save
			//shufps xmm0,xmm2,0xEE  ; z[] in xmm0 now
			//shufps xmm6,xmm2,0x44  ; y[] in xmm6 now

			//unpcklps xmm1,xmm4
			//unpcklps xmm3,xmm5
			//movaps xmm7,xmm1
			//shufps xmm1,xmm3,0xBB  ; x[] in xmm1 now
			//shufps xmm7,xmm3,0x11  ; s[] in xmm7 now

			//movaps xmm3,xmm0
			//movaps xmm2,xmm6
			//movaps xmm0,xmm7
	}
}

// best time (precise) = 346
// best time (approx) = 150
void normalize_4q(float *q)
{
	// <assembler using="eax, xmm0..xmm3"></assembler>
	__asm
	{
		mov eax, q                     ;// load q
		movaps xmm0, [eax]             ;// 
		add eax, 16                    ;// 
		movaps xmm1, [eax]             ;// 
		add eax, 16                    ;// 
		movaps xmm2, [eax]             ;// 
		add eax, 16                    ;// 
		movaps xmm3, [eax]             ;// 
	}

	transpose_xmm0123();

	__asm
	{
		;// compute square
		mulps xmm0,xmm0                ;// 
		mulps xmm1,xmm1                ;// 
		mulps xmm2,xmm2                ;// 
		mulps xmm3,xmm3                ;// 

		;// similar to dot product
		addps xmm0,xmm1                ;// 
		addps xmm0,xmm2                ;// 
		addps xmm0,xmm3                ;// 

#ifndef APPROX
		sqrtps xmm0,xmm0               ;// u[] is stored in xmm0
#else
		rsqrtps xmm0,xmm0              ;// 1/u[]
#endif
		;// copy u[] and then transpose
		movaps xmm1,xmm0
			movaps xmm2,xmm0
			movaps xmm3,xmm0
	}

	transpose_xmm0123();

	// <assembler using="eax, xmm0..xmm3"></assembler>
	__asm
	{
		mov eax, q                     ;// reload q
		movaps xmm4, [eax]             ;// 
		add eax, 16                    ;// 
		movaps xmm5, [eax]             ;// 
		add eax, 16                    ;// 
		movaps xmm6, [eax]             ;// 
		add eax, 16                    ;// 
		movaps xmm7, [eax]             ;// 

#ifndef APPROX
		divps xmm4,xmm0                ;// 
		divps xmm5,xmm1                ;// 
		divps xmm6,xmm2                ;// 
		divps xmm7,xmm3                ;// 
#else
		mulps xmm4,xmm0                ;// 
		mulps xmm5,xmm1                ;// 
		mulps xmm6,xmm2                ;// 
		mulps xmm7,xmm3                ;// 
#endif

		mov eax, q                     ;// store q
		movaps [eax], xmm4             ;// 
		add eax, 16                    ;// 
		movaps [eax], xmm5             ;// 
		add eax, 16                    ;//  
		movaps [eax], xmm6             ;// 
		add eax, 16                    ;//  
		movaps [eax], xmm7             ;// 
	}
}
#endif

void Quaternion::Square(Quaternion *q)
{
	const float *src = q->coord;
	const float *dest = this->coord;
#ifdef SSSE3
	__asm
	{
		mov eax,src
		movaps xmm0,xmmword ptr [eax]
		mulps xmm0,xmm0
		mov eax,dest
		movaps xmmword ptr [eax],xmm0
	}
#else
	for (int i = 0; i < 4; i++)
	{
	    this->coord[i] = q->coord[i]*q->coord[i];
	}
#endif
}

void Quaternion::Integrate(Quaternion& derivative, const FP_DATATYPE dt)
{
	const void *dest = this->coord;
#ifdef SSSE3_
	__asm
	{
		movss xmm1,dt                            ;// xmm1 <= dt
		shufps xmm1,xmm1,0x00

		mov eax,derivative
			movaps xmm0, xmmword ptr [eax]       ;// xmm0 <= derivative
			mulps xmm0, xmm1                     ;// xmm0 <= derivative*dt
			mov eax,dest
			addps xmm0, xmmword ptr [eax]        ;// xmm0 <= derivative*dt + destination
			movaps xmmword ptr [eax], xmm0
	}
#else
	coord[S] += derivative[S] * dt;
	coord[VX] += derivative[VX] * dt;
	coord[VY] += derivative[VY] * dt;
	coord[VZ] += derivative[VZ] * dt;
#endif
}